function [IMUPar, accPSBCond, accPS0BCond, gyroPSBCond, gyroPS0BCond] = genrandimupar(nSamp, nonLinearSFErr, crsDepErrIsZero)
%GENRANDIMUPAR 生成测试用随机惯组参数

% NOTE: 由于外层迭代可能产生正反馈，仿真误差受以下因素影响较大：
% 1. PS0B的条件数
% 2. 加速度计组杆臂
% 3. 陀螺g敏感项
% 4. 三个标度因数标称值的比值

if nargin < 3
    crsDepErrIsZero = false;
end

IMUPar.acc.KScal0 = randi([1e4 5e4], [3 nSamp]); % 1e4~5e4^/s/g
IMUPar.acc.PS0B = randi([-3 3], [3 3 nSamp]);
IMUPar.acc.dfBiasS = randi([-10, 10], [3 nSamp]) * 1e3; % 10mg
IMUPar.acc.dKScalP(:, 1, :) = randi([-1000, 1000], [3 1 nSamp]); % 1ppt
IMUPar.acc.dKScalN(:, 1, :) = randi([-1000, 1000], [3 1 nSamp]); % 1ppt
if nonLinearSFErr
    IMUPar.acc.dKScalP(:, 2, :) = randi([-30, 30], [3 1 nSamp]); % 30μg/g^2
    IMUPar.acc.dKScalN(:, 2, :) = randi([-30, 30], [3 1 nSamp]); % 30μg/g^2
    IMUPar.acc.dKScalP(:, 3, :) = randi([-100, 100], [3 1 nSamp]) / 100; % 1μg/g^3
    IMUPar.acc.dKScalN(:, 3, :) = randi([-100, 100], [3 1 nSamp]) / 100; % 1μg/g^3
end
IMUPar.acc.dPSS0 = randi([-10, 10], [3 3 nSamp]) * 1e3; % 10mrad
IMUPar.acc.dfQuantS = 1./IMUPar.acc.KScal0 .* randi([-10, 10], [3 nSamp]) / 10;
if crsDepErrIsZero
    IMUPar.acc.lB = zeros(3, 3, nSamp);
    IMUPar.acc.KAniso = zeros(3, nSamp);
else
    IMUPar.acc.lB = randi([-100 100], [3 3 nSamp]); % 100mm
    IMUPar.acc.KAniso = randi([-10, 10], [3 nSamp]); % 10μg/(°/s)^2
end
IMUPar.acc = convaccpar(IMUPar.acc);
accPSBCond = NaN(nSamp, 1);
accPS0BCond = NaN(nSamp, 1);
for iSamp=1:nSamp
    [IMUPar.acc.PS0B(:, :, iSamp), IMUPar.acc.dPSS0(:, :, iSamp), accPSBCond(iSamp), accPS0BCond(iSamp)] = condnormPBS(IMUPar.acc.PS0B(:, :, iSamp), IMUPar.acc.dPSS0(:, :, iSamp));
    for jAxis=1:3
        IMUPar.acc.CBA(:, :, jAxis, iSamp) = accrf2bodyrf(IMUPar.acc.PS0B(:, :, iSamp), jAxis)';
    end
end

IMUPar.gyro.KScal0 = randi([1e5 5e5], [3 nSamp]); % 1e5~5e5^/″
IMUPar.gyro.PS0B = randi([-3 3], [3 3 nSamp]);
IMUPar.gyro.domegaIBBiasS = randi([-30, 30], [3 nSamp]); % 30°/h
IMUPar.gyro.dKScalP(:, 1, :) = randi([-1000, 1000], [3 1 nSamp]); % 1ppt
IMUPar.gyro.dKScalN(:, 1, :) = randi([-1000, 1000], [3 1 nSamp]); % 1ppt
if nonLinearSFErr
    IMUPar.gyro.dKScalP(:, 2, :) = randi([-10, 10], [3 1 nSamp]); % 10ppm/(°/s)
    IMUPar.gyro.dKScalN(:, 2, :) = randi([-10, 10], [3 1 nSamp]); % 10ppm/(°/s)
    IMUPar.gyro.dKScalP(:, 3, :) = randi([-1, 1], [3 1 nSamp]) / 100; % 0.01ppm/(°/s)^2
    IMUPar.gyro.dKScalN(:, 3, :) = randi([-1, 1], [3 1 nSamp]) / 100; % 0.01ppm/(°/s)^2
end
IMUPar.gyro.dPSS0 = randi([-10, 10], [3 3 nSamp]) * 1e3; % 10mrad
IMUPar.gyro.domegaIBQuantS = 1./IMUPar.gyro.KScal0 .* randi([-10, 10], [3 nSamp]) / 10;
if crsDepErrIsZero
    IMUPar.gyro.DGSens = zeros(3, 3, nSamp);
else
    IMUPar.gyro.DGSens = randi([-10, 10], [3 3 nSamp]); % 10°/h/g
end
IMUPar.gyro = convgyropar(IMUPar.gyro);
gyroPSBCond = NaN(nSamp, 1);
gyroPS0BCond = NaN(nSamp, 1);
for iSamp=1:nSamp
    [IMUPar.gyro.PS0B(:, :, iSamp), IMUPar.gyro.dPSS0(:, :, iSamp), gyroPSBCond(iSamp), gyroPS0BCond(iSamp)] = condnormPBS(IMUPar.gyro.PS0B(:, :, iSamp), IMUPar.gyro.dPSS0(:, :, iSamp));
end
end

function [PS0B, dPSS0, PSBCond, PS0BCond] = condnormPBS(PS0B, dPSS0)
if cond(PS0B) > 5 % NOTE: 若S系正交则PS0B条件数为1，斜交时条件数可能增大
    PS0B = eye(3); % 将条件数大的替换为单位阵
else
    PS0B = PS0B ./ repmat(rss_cg(PS0B, 1), 3, 1); % 各列归一化
end
PSB = PS0B * (eye(3)+dPSS0);
PSB = PSB ./ repmat(rss_cg(PSB, 1), 3, 1); % 各列归一化
dPSS0 = PS0B\(PSB-PS0B);
PSBCond = cond(PSB);
PS0BCond = cond(PS0B);
end

function [CAB] = accrf2bodyrf(PS0B, inputAxisInd)
% 加速度计坐标系A系的X轴与S0系第inputAxisInd轴重合，Y轴与S0系第inputAxisInd轴及第inputAxisInd+1轴垂直
CAB(:, 1) = PS0B(:, inputAxisInd);
inputAxisIndP1 = inputAxisInd + 1;
if inputAxisIndP1 > 3
    inputAxisIndP1 = 1;
end
CAB(:, 2) = cross(PS0B(:, inputAxisInd), PS0B(:, inputAxisIndP1));
CAB(:, 2) = CAB(:, 2) / norm(CAB(:, 2));
CAB(:, 3) = cross(CAB(:, 1), CAB(:, 2));
end